using UnityEngine;

namespace Kayac
{
	public class PidControllerRotation : IPidController<Vector3>
	{
		private Quaternion localToNatural; // 局部坐标系到自然坐标系的转换
		private PidSettings settings;      // PID参数配置
		private PidState<Vector3> state;   // PID状态
		private bool started;              // 是否已开始运行

		// 状态访问器
		public PidState<Vector3> State => state;
		public Vector3 ErrorSum => state.eSum;
		public Vector3 PrevError => state.prevE;

		public PidControllerRotation(PidSettings settings, Quaternion localToNatural)
		{
			this.settings = settings;
			this.localToNatural = localToNatural;
			state = new PidState<Vector3>(Vector3.zero, Vector3.zero);
			started = false;
		}

		public PidControllerRotation(PidSettings settings)
			: this(settings, Quaternion.identity)
		{
		}

		// 更新PID控制器状态并返回控制输出
		public Vector3 Update(Quaternion x, Quaternion r, float deltaTime, float updateRatio = 1f)
		{
			// 计算当前姿态到目标姿态的相对旋转
			Quaternion rel = r * Quaternion.Inverse(x);

			// 将四元数转换为旋转向量（局部坐标系）
			Vector3 e_local = Log(rel);

			// 转换到自然坐标系
			Vector3 e = localToNatural * e_local;

			// 初始化状态（如果是首次运行）
			if (!started)
			{
				state.prevE = e;
				state.eSum = Vector3.zero;
				started = true;
			}

			// 计算PID各项
			Vector3 p = settings.kp * e;
			state.eSum += e * deltaTime * updateRatio;
			Vector3 i = settings.ki * state.eSum;
			Vector3 d = settings.kd * (e - state.prevE) / deltaTime;

			// 更新状态
			state.prevE = e;

			return p + i + d;
		}

		// 更新PID参数设置
		public void SetSettings(PidSettings settings)
		{
			this.settings = settings;
		}

		// 重置控制器状态
		public void Reset()
		{
			started = false;
			state.prevE = Vector3.zero;
			state.eSum = Vector3.zero;
		}

		// 恢复保存的状态
		public void Restore(PidState<Vector3> state)
		{
			this.state = state;
			started = true;
		}

		// 将四元数转换为旋转向量（对数映射）
		private static Vector3 Log(Quaternion q)
		{
			// 确保使用最小旋转路径（w为正）
			if (q.w < 0)
			{
				q = new Quaternion(-q.x, -q.y, -q.z, -q.w);
			}

			// 处理单位四元数的情况
			if (q.w > 0.9999f)
			{
				return new Vector3(q.x * 2f, q.y * 2f, q.z * 2f);
			}

			// 计算旋转角度和系数
			float sinHalfTheta = Mathf.Sqrt(1 - q.w * q.w);
			float halfTheta = Mathf.Atan2(sinHalfTheta, q.w);
			float coeff = 2f * halfTheta / sinHalfTheta;

			return new Vector3(q.x * coeff, q.y * coeff, q.z * coeff);
		}
	}
}